//-----------------------------------------------------------------------
//
//  Written Dec-2007 by Trevor Taylor, Software Technology, Australia
//
//  $File: StingerDriveByWire.cs $ $Revision: 1 $
//-----------------------------------------------------------------------
using Microsoft.Ccr.Core;
using Microsoft.Ccr.Adapters.WinForms;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using System;
using System.Collections.Generic;
using System.Security.Permissions;
using xml = System.Xml;
//using drive = Microsoft.Robotics.Services.Drive.Proxy;
using W3C.Soap;
//using ProMRDS.Robotics.StingerDriveByWire.Properties;
using System.ComponentModel;
using System.Threading;

// For Stinger
using SerializerSvc = RoboticsConnection.SerializerServices.Proxy;
using PwmMotorProxy = RoboticsConnection.SerializerServices.Motor.Proxy;
using Gp2d12Proxy = RoboticsConnection.SerializerServices.Gp2d12.Proxy;
using GpioProxy = RoboticsConnection.SerializerServices.Gpio.Proxy;
using RoboticsConnection.SerializerServices.Common.State.Proxy;
using ds = Microsoft.Dss.Services.Directory;
using coord = Microsoft.Robotics.Services.Coordination.Proxy;
using cp = Microsoft.Dss.Services.ControlPanel;
using System.Windows.Forms;


namespace ProMRDS.Robotics.StingerDriveByWire
{
    [DisplayName("Stinger Drive By Wire")]
    [Description("Autonomous Drive By Wire example for Stinger robot")]
    [Contract(Contract.Identifier)]
    public class StingerDriveByWire : DsspServiceBase
    {
        #region Globals

        // Set an OPTIONAL initial state partner
        // NOTE: If the config file is NOT specified in the manifest, then the
        // file will be created in the MRDS root directory. If it is listed in the
        // manifest, then it will be created in the same directory as the manifest.
        [InitialStatePartner(Optional = true, ServiceUri = "StingerDriveByWire.Config.xml")]
        private StingerDriveByWireState _state = new StingerDriveByWireState();
        
        // The main port for this service and its name
        [ServicePort("/StingerDriveByWire", AllowMultipleInstances=false)]
        private StingerDriveByWireOperations _mainPort = new StingerDriveByWireOperations();

//        [Partner("Drive", Contract = drive.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
//        private drive.DriveOperations _drivePort = new drive.DriveOperations();
//        private drive.DriveOperations _driveNotify = new drive.DriveOperations();

        // The Control Panel partner is only used to shutdown the DSS node
        [Partner("ControlPanel", Contract = cp.Contract.Identifier, CreationPolicy =
              PartnerCreationPolicy.UseExisting, Optional = false)]
        cp.ControlPanelOperations _cpPort = new
              Microsoft.Dss.Services.ControlPanel.ControlPanelOperations();

        // These are required for the Windows Form
        private static DispatcherQueue _DispatcherQueue;
        private static WinFormsServicePort _WinFormsPort;
        private StingerDriveByWireForm _driveForm;

        // Flag to indicate if the timer should be running
        // Only necessary when Wandering
        private bool _timerRunning = false;

        // Main constructor
        public StingerDriveByWire(DsspServiceCreationPort creationPort) : 
                base(creationPort)
        {

        }

        #endregion

        #region Constants

        // Thresholds for the IR sensors
        // NOTE: These values are in CENTIMETERS and depend on the config
        // settings for the IR sensors being set to Metric. Also, the
        // Stinger does not turn around its center -- the back end hangs out!
        // This means that it really needs about 30cm clearance to avoid
        // banging its backside on a wall as it turns. A value of 20 for
        // the Front Threshold is probably too low, but in limited space
        // you don't want to set it too high. Bear in mind that the GP2D12
        // can only measure down to 10cm and then it starts to go UP again!
        private const double FRONT_THRESHOLD = 20;
        private const double LEFT_THRESHOLD = 30;
        private const double RIGHT_THRESHOLD = 30;

        // Timer interval for the Wander behavior
        private const int WANDER_INTERVAL = 100;

        // Number of timer "ticks" to execute respective behaviors
        // Multiply these counters times the Wander Interval to get
        // the amount of time spent in each behavior
        private const int TURN_COUNT = 10;
        private const int VEER_COUNT = 10;
        private const int BACKUP_COUNT = 10;

        // Motor power to use for various modes
        // The Stinger power settings range from -100 to +100,
        // but don't include a sign here
        // NOTE: In limited space, don't set these higher than
        // about 50. This little sucker is fast!
        private const int DRIVE_POWER = 50;
        private const int TURN_POWER = 50;
        private const int VEER_POWER = 25;
        private const int BACKUP_POWER = 50;

        #endregion

        #region Partners and Ports

        // This partnership was only added in an attempt to shutdown
        // the serializer, i.e. the serial port connection
        [Partner("Serializer",
            Contract = SerializerSvc.Contract.Identifier,
            CreationPolicy = PartnerCreationPolicy.UsePartnerListEntry)]
        SerializerSvc.SerializerOperations _serializerPort = new SerializerSvc.SerializerOperations();

        // The GPIO is only used to beep and flash the lights
        [Partner("Gpio",
            Contract = GpioProxy.Contract.Identifier,
            CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
        GpioProxy.GpioOperations _gpioPort = new GpioProxy.GpioOperations();
        GpioProxy.GpioOperations _gpioNotifyPort = new GpioProxy.GpioOperations();

        // Notice that we only partner with one motor because you can send
        // a request to a motor service with either ID and it will work
        [Partner("Motor1",
            Contract = PwmMotorProxy.Contract.Identifier,
            CreationPolicy = PartnerCreationPolicy.UsePartnerListEntry)]
        PwmMotorProxy.MotorOperations _motorPort = new PwmMotorProxy.MotorOperations();
        PwmMotorProxy.MotorOperations _motorNotifyPort = new PwmMotorProxy.MotorOperations();
        // For unsubscribing
        Port<Shutdown> _motorShutdownPort = new Port<Shutdown>();

        // Ports for the InfraRed sensors
        // Notice that there is NO Partner attribute on this port
        // See below for subscription
        Gp2d12Proxy.Gp2d12Operations _gp2d12Port;
        Gp2d12Proxy.Gp2d12Operations _gp2d12NotifyPort;
        // For unsubscribing
        Port<Shutdown> _gp2d12ShutdownPort = new Port<Shutdown>();

        // Port to receive Directory service notifications over for subscribing...
        ds.DirectoryPort _directoryPort;
        ds.DirectoryPort _directoryNotifyPort = new ds.DirectoryPort();

        #endregion

        #region Initialization

        // Standard Start method ...
        protected override void Start()
        {
            base.Start();

            // Create a new state if the config file was not found
            if (_state == null)
            {
                _state = new StingerDriveByWireState();
                // Assume by default that we want to use the motors!
                // Note that this will not override the config file
                _state.MotorEnabled = true;
                // Just to be sure ...
                _state.Headless = false;
                // Do any other initialization of the state here ...
            }

            // Initially we are not in any particular wander mode
            // regardless of what might have been saved in the config file
            _state.WanderMode = WanderModes.None;

            // Make sure that the IR sensor values are all zero
            // The Wander behavior will not start unless there are non-zero
            // values for all of the sensors. This allows the IR services
            // to start up and also handles the case where a sensor does
            // not work so there are no updates.
            // NOTE: When starting up, it might be necessary to give the
            // robot a little "nudge". You can do this by moving it a bit,
            // or by waving your hand in front of an IR sensor. This is
            // necessary because the IR services only send updates when
            // something changes, and initially there is no data.
            _state.IRLeft = _state.IRFront = _state.IRRight = 0;

            // If we are NOT Headless, then create the WinForm UI
            if (!_state.Headless)
            {
                // Cannot use the WinFormsServicePort under CF, so build
                // one instead. This code snippet is from Olivier Bloch
                // from Microsoft in France and replaces the following line:
                //WinFormsServicePort.Post(new RunForm(StartForm));

                // Create the Dispatcher that will execute our delegates that we add to the 
                // DispatcherQueue
                Dispatcher dispatcher = new Dispatcher(0, "WinForms Dispatcher");
                _DispatcherQueue = new DispatcherQueue("WinForms DispatcherQueue", dispatcher);

                // Bind the DispatcherQueue to the WinFormsAdapter and
                // create the WinFormServicePort
                _WinFormsPort = WinFormsAdapter.Create(_DispatcherQueue);

                // Use the WinFormsServicePort to create a MainForm
                _WinFormsPort.Post(new RunForm(StartForm));
            }

//          _drivePort.Subscribe(_driveNotify);
//          Activate(Arbiter.Receive<drive.Update>(true, _driveNotify, NotifyDriveUpdate));
            SubscribeToPwmMotorService();

            // We need the IR sensors for wandering
            SubscribeToIRSensors();

            // Beep and Flash LED to indicate connection
            _gpioPort.SetPin(SerializerIoPin.Pin0, PinState.On);
            _gpioPort.SetPin(SerializerIoPin.Pin3, PinState.On);
//            _gpioPort.SetPin(SerializerIoPin.Pin5, PinState.On);
            Thread.Sleep(300);
            _gpioPort.SetPin(SerializerIoPin.Pin0, PinState.Off);
            _gpioPort.SetPin(SerializerIoPin.Pin3, PinState.Off);
//            _gpioPort.SetPin(SerializerIoPin.Pin5, PinState.Off);

        } // End Start()


        // Create the new WinForm
        private System.Windows.Forms.Form StartForm()
        {
            StingerDriveByWireForm form = new StingerDriveByWireForm(_mainPort);
            // Keep a handle to the Form
            _driveForm = form;
            return form;
        }

        #endregion

        #region Subscriptions

        // Subscribe to the Motor service
        void SubscribeToPwmMotorService()
        {
            //_motorPort.Subscribe(_motorNotifyPort);
            PwmMotorProxy.Subscribe subscribe = new PwmMotorProxy.Subscribe(new SubscribeRequestType());
            subscribe.NotificationPort = _motorNotifyPort;
            subscribe.NotificationShutdownPort = _motorShutdownPort;
            _motorPort.Post(subscribe);

            // Set up a handler
            Activate(Arbiter.Receive<PwmMotorProxy.SetMotorPower>
                (true, _motorNotifyPort, ReplaceMotorHandler));

            Console.WriteLine("Subscribed to Motor Service");
        }


        // Subscribe to the IR sensors
        // NOTE: This is a round-about process because there are three
        // IR sensors and we want to handle them all together
        void SubscribeToIRSensors()
        {
            // Create a Directory Service port to receive notifications
            _directoryPort = ServiceForwarder<ds.DirectoryPort>(ServicePaths.InstanceDirectory);

            // Send a subscription to the Directory service for the type of
            // contract that we are interested in
            SendSubscriptionToDirectory(Gp2d12Proxy.Contract.Identifier);

            Console.WriteLine("Listening for subscriptions from Directory Service");
            // Listen for Directory insertions
            Activate(
                Arbiter.Receive<ds.Insert>(true, _directoryNotifyPort, InsertNotificationHandler));
        }

        // Send a subscription to the Directory Service for a type of Contract
        private void SendSubscriptionToDirectory(string identifier)
        {
            ds.SubscribeRequestType subBody =
                new ds.SubscribeRequestType(null,
                new ServiceInfoType(identifier));

            // Create a subscribe message, and fill the body in...
            ds.Subscribe sub = new ds.Subscribe(subBody);
            sub.NotificationPort = _directoryNotifyPort;

            // Send the message to the Directory
            _directoryPort.Post(sub);

            // Set up a one-time receiver
            Activate(
               Arbiter.Choice(
                   Arbiter.Receive<SubscribeResponseType>(false, sub.ResponsePort,
                       delegate(SubscribeResponseType response)
                       {

                       }),
                   Arbiter.Receive<Fault>(false, sub.ResponsePort,
                       delegate(Fault f)
                       {
                           LogError("Subscribe to directory service failed for " + identifier);
                       })
               )
           );
        }


        // Process Directory insertions
        void InsertNotificationHandler(ds.Insert insert)
        {
            // See if the new service in the Directory is one that we are
            // interested in
            if (insert.Body.Record.Contract == Gp2d12Proxy.Contract.Identifier)
            {
                // Create a new notification port for IR messages
                _gp2d12NotifyPort = new Gp2d12Proxy.Gp2d12Operations();
                // Get a service forwarder so we can send messages to the IR service
                _gp2d12Port = ServiceForwarder<Gp2d12Proxy.Gp2d12Operations>(insert.Body.Record.Service);
                Gp2d12Proxy.Subscribe subscribe = new Gp2d12Proxy.Subscribe(new SubscribeRequestType());
                subscribe.NotificationPort = _gp2d12NotifyPort;
                // Set the Shutdown port too so we can unsubscribe later
                subscribe.NotificationShutdownPort = _gp2d12ShutdownPort;
                // Now subscribe to the IR service
                _gp2d12Port.PostUnknownType(subscribe);

                // Set up a persistent receiver
                MainPortInterleave.CombineWith(
                    new Interleave(
                        new TeardownReceiverGroup(),
                        new ExclusiveReceiverGroup(
                            Arbiter.Receive<Gp2d12Proxy.UpdateReading>
                                (true, _gp2d12NotifyPort, ReplaceIRHandler)
                        ),
                        new ConcurrentReceiverGroup()
                    )
                );

                // Can't have a "free running" receiver because the
                // IR sensors might cause changes to the speed
                //Activate(Arbiter.Receive<Gp2d12Proxy.UpdateReading>
                //    (true, _gp2d12NotifyPort, ReplaceIRHandler));

                Console.WriteLine("Subscribed to Gp2d12 Service");
            }
        }

        #endregion

        #region General Handlers

        /// <summary>
        /// Get Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> GetHandler(Get get)
        {
            get.ResponsePort.Post(_state);
            yield break;
        }


        /// <summary>
        /// Replace Handler
        /// </summary>
        /// <param name="replace"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> ReplaceHandler(Replace replace)
        {
            _state = replace.Body;
            replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
            yield break;
        }

        /// <summary>
        /// Load Handler
        /// </summary>
        /// <param name="load"></param>
        /// <returns></returns>
        /// <remarks>Called when the Form has initialized</remarks>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> LoadHandler(Load load)
        {
            // Some people use this as a trigger to start other parts
            // of the initialization. However, the Form can be loaded
            // long before the rest of the services have completed their
            // own initialization so it is not a guarantee.

            // At this stage, you might need to transfer initial state
            // values to the Form UI

            // Set the initial value of the Wander checkbox
            _WinFormsPort.FormInvoke(
                delegate()
                {
                    _driveForm.SetWander(_state.WanderEnabled);
                }
            );
            yield break;
        }

        /// <summary>
        /// Enable Wander Handler
        /// </summary>
        /// <param name="wander"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> EnableWanderHandler(Wander wander)
        {
            _state.WanderEnabled = wander.Body.Status;

            if (!_state.WanderEnabled)
            {
                // Not wandering any more ...

                // Stop the motors!
                SetMotors(0, 0);
                // Turn off the timer because it is not required
                _timerRunning = false;
                // Revert back to no mode, otherwise we might start up
                // in some strange state!
                _state.WanderMode = WanderModes.None;
            }

            yield break;
        }

        /// <summary>
        /// Quit Handler
        /// </summary>
        /// <param name="quit"></param>
        /// <returns></returns>
        /// <remarks>Called when the user presses the Exit button</remarks>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> QuitHandler(Quit quit)
        {
            // Stop the motors immediately
            SetMotors(0, 0);

            // Turn off the timer
            _timerRunning = false;

            // Save the current state (which records the WanderEnabled flag)
            SaveState(_state);

            Console.WriteLine("Shutting Down ...");

            // Unsubscribe from the motor service
            Shutdown shutdown = new Shutdown();
            _motorShutdownPort.Post(shutdown);
            _motorPort.PostUnknownType(new DsspDefaultDrop(DropRequestType.Instance));

            // Shut down the IR Sensors
            _gp2d12ShutdownPort.Post(shutdown);
            _gp2d12ShutdownPort.Post(shutdown);
            _gp2d12ShutdownPort.Post(shutdown);

            // Force the Serializer to exit so that it releases the COM port
            _serializerPort.PostUnknownType(new DsspDefaultDrop(DropRequestType.Instance));

            // Try shutting down WinForms
            if (!_state.Headless)
                _WinFormsPort.PostUnknownType(new Shutdown());

            // Tell ourselves to shutdown
            _mainPort.PostUnknownType(new DsspDefaultDrop(DropRequestType.Instance));

            Console.WriteLine("Tell Control Panel to Drop Process ...");

            // Tell Control Panel to shut up shop too
            // NOTE: This rarely works on Pocket PC :-(
            _cpPort.Post(new Microsoft.Dss.Services.ControlPanel.DropProcess());
#if URT_MINCLR
            Thread.Sleep(500);
            System.Diagnostics.Process p = System.Diagnostics.Process.GetCurrentProcess();
            p.Kill();
#endif
            yield break;
        }

        #endregion

        #region Motor Handlers

        /// <summary>
        /// Motion Handler
        /// </summary>
        /// <param name="motion"></param>
        /// <returns></returns>
        /// <remarks>Handles all motion requests from the Form or Wander Behavior</remarks>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> MotionHandler(MotionCommand motion)
        {
            SetMotors(motion.Body.LeftPower, motion.Body.RightPower);
            yield break;

            /*
            drive.SetDrivePowerRequest request = new drive.SetDrivePowerRequest();
            request.LeftWheelPower = 0.75;
            request.RightWheelPower = 0.75;

            yield return Arbiter.Choice(
                _drivePort.SetDrivePower(request),
                delegate(DefaultUpdateResponseType response) { },
                delegate(Fault fault)
                {
                    LogError(null, "Unable to drive", fault);
                }
            );
             */
        }


        // Common routine to handle all motor commands
        // NOTE: This should not be called directly. Post a MotionCommand
        // to the main port so that the requests will be queued (it is an
        // Exclusive handler). There are a couple of places where this rule
        // is violated because it is safe to do so.
        void SetMotors(int motor1Power, int motor2Power)
        {
            if (_state.MotorEnabled)
            {
                // Add a coordination header to the motor requests
                // so that advanced motor implementations can
                // coordinate the individual motor reqests.
                // Note that there are TWO items in the coordination.
                coord.ActuatorCoordination coordination = new coord.ActuatorCoordination();
                coordination.Count = 2;

                // Set up the requests
                PwmMotorProxy.SetMotorPowerRequest req1 = new PwmMotorProxy.SetMotorPowerRequest();
                req1.Id = 1;
                req1.TargetPower = motor1Power;
                PwmMotorProxy.SetMotorPower leftSmp = new PwmMotorProxy.SetMotorPower(req1);
                leftSmp.AddHeader(coordination);

                PwmMotorProxy.SetMotorPowerRequest req2 = new PwmMotorProxy.SetMotorPowerRequest();
                req2.Id = 2;
                req2.TargetPower = motor2Power;
                PwmMotorProxy.SetMotorPower rightSmp = new PwmMotorProxy.SetMotorPower(req2);
                rightSmp.AddHeader(coordination);

                // Now post to both motors as quickly as possible just in case
                // the coordination does not work
                _motorPort.Post(leftSmp);
                _motorPort.Post(rightSmp);
            }
            else
            {
                // The motors are disabled (for debugging) but show the user
                // what would have happened
                Console.WriteLine("Dummy Motors: {0},{1}", motor1Power, motor2Power);
            }
        }


        // This might be an option later -- to enable/disable the motors
        // NOTE: For a normal Differential Drive you have to enable the motors.
        // This is not the case for the Serializer.
        void EnableMotor()
        {
            _state.MotorEnabled = true;

            /*
            drive.EnableDriveRequest request = new drive.EnableDriveRequest();
            request.Enable = true;

            return Arbiter.Choice(
                _drivePort.EnableDrive(request),
                delegate(DefaultUpdateResponseType response) { },
                delegate(Fault fault)
                {
                    LogError(null, "Unable to enable motor", fault);
                }
            );
             */
        }

        /*
        private void NotifyDriveUpdate(drive.Update update)
        {
            StingerDriveByWireState state = new StingerDriveByWireState();
            state.MotorEnabled = update.Body.IsEnabled;

            _mainPort.Post(new Replace(state));
        }
         */

        // Handler for notification messages from the Motors
        void ReplaceMotorHandler(PwmMotorProxy.SetMotorPower r)
        {
            // Take this command out if you want "quiet" operation
            // This is especially important on WinCE where the console is an
            // emulation that is very slow to update
            // if (!_state.Headless)
            //     Console.WriteLine("ReplaceMotorHandler: Motor: {0}, Speed: {1}", r.Body.Id, r.Body.TargetPower);
            /*
            // Display on the Form too??? Maybe in the future ...
            _WinFormsPort.FormInvoke(
                delegate()
                {
                    _driveForm.SetPwmMotorSpeed(r.Body.Id, r.Body.TargetPower);
                }
             );
             */
        }

        #endregion

        #region Wander

        // Handle IR Sensor updates
        // This handler is essential to the operation of the Wander behavior.
        // However, it does not execute the behavior directly. Instead, the
        // Wander behavior runs on a timer. This gives a far greater chance
        // that all three of the IR sensors will have updated their values.
        // Otherwise, we would be reevaluating the status after every new
        // individual value, and this is not a good idea.
        public void ReplaceIRHandler(Gp2d12Proxy.UpdateReading ur)
        {
            // Save the value
            switch (ur.Body.Pin)
            {
                case SerializerIoPin.Pin0:
                    _state.IRLeft = ur.Body.Distance;
                    break;

                case SerializerIoPin.Pin1:
                    _state.IRFront = ur.Body.Distance;
                    break;

                case SerializerIoPin.Pin2:
                    _state.IRRight = ur.Body.Distance;
                    break;

                default:
                    break;
            }

            // Display the value if we have a GUI
            if (!_state.Headless)
            {
                _WinFormsPort.FormInvoke(
                    delegate()
                    {
                        _driveForm.SetIRReadings((int)ur.Body.Pin, ur.Body.Distance);
                    }
                );
            }

            // Finally, if the timer is not going then start it now.
            // This ensures that the timer does not start until IR data
            // is arriving. Otherwise the Wander behavior might try to
            // process IR values before they are valid and this would
            // result in some strange behavior initially.
            if (_state.WanderEnabled && !_timerRunning)
            {
                if (_state.IRLeft != 0 && _state.IRFront != 0 && _state.IRRight != 0)
                {
                    // Indicate that the timer is active and call the Wander
                    // routine to kick it off immediately with the new IR
                    // data that has just arrived
                    _timerRunning = true;
                    Wander(System.DateTime.Now);
                }
            }
        }


        // Show the current Wander State (when it changes)
        // There are various ways to do this:
        //   Using Console.WriteLine is brute force, but it works; or
        //   Using one of the Logging functions would be better; or
        //   Displaying info on the Form.
        // Putting all the code here allows you to easily comment
        // out all of the messages in one hit, i.e. when running
        // in Headless mode.
        void DisplayWanderState()
        {
            if (!_state.Headless)
            {
                switch (_state.WanderMode)
                {
                    case WanderModes.DriveStraight:
                        _WinFormsPort.FormInvoke(
                            delegate()
                            {
                                _driveForm.SetWanderMode("Forward");
                            }
                        );
                        //Console.WriteLine("=> Drive Straight");
                        break;
                    case WanderModes.BackUp:
                        _WinFormsPort.FormInvoke(
                            delegate()
                            {
                                _driveForm.SetWanderMode("Back Up");
                            }
                        );
                        //Console.WriteLine("=> Back Up");
                        break;
                    case WanderModes.TurnLeft:
                        _WinFormsPort.FormInvoke(
                            delegate()
                            {
                                _driveForm.SetWanderMode("Turn Left");
                            }
                        );
                        //Console.WriteLine("=> Turn Left");
                        break;
                    case WanderModes.TurnRight:
                        _WinFormsPort.FormInvoke(
                            delegate()
                            {
                                _driveForm.SetWanderMode("Turn Right");
                            }
                        );
                        //Console.WriteLine("=> Turn Right");
                        break;
                    case WanderModes.VeerLeft:
                        _WinFormsPort.FormInvoke(
                            delegate()
                            {
                                _driveForm.SetWanderMode("Veer Left");
                            }
                        );
                        //Console.WriteLine("=> Veer Left");
                        break;
                    case WanderModes.VeerRight:
                        _WinFormsPort.FormInvoke(
                            delegate()
                            {
                                _driveForm.SetWanderMode("Veer Right");
                            }
                        );
                        //Console.WriteLine("=> Veer Right");
                        break;
                    case WanderModes.None:
                        _WinFormsPort.FormInvoke(
                            delegate()
                            {
                                _driveForm.SetWanderMode("");
                            }
                        );
                        //Console.WriteLine("=> None");
                        break;
                    default:
                        _WinFormsPort.FormInvoke(
                            delegate()
                            {
                                _driveForm.SetWanderMode("Invalid Mode!");
                            }
                        );
                        //Console.WriteLine("=> Invalid Wander Mode!");
                        break;
                }
            }
        }


        // Routines to implement motions
        // NOTE: These INITIATE a motion and should not be called repeatedly!
        void DriveStraight()
        {
            _state.WanderMode = WanderModes.DriveStraight;
            _state.WanderCounter = 0;
            _mainPort.MotionCommand(DRIVE_POWER, DRIVE_POWER);
            DisplayWanderState();
        }

        void BackUp()
        {
            _state.WanderMode = WanderModes.BackUp;
            _state.WanderCounter = BACKUP_COUNT;
            _mainPort.MotionCommand(-BACKUP_POWER, -BACKUP_POWER);
            DisplayWanderState();
        }

        void TurnLeft()
        {
            _state.WanderMode = WanderModes.TurnLeft;
            _state.WanderCounter = TURN_COUNT;
            _mainPort.MotionCommand(-TURN_POWER, TURN_POWER);
            DisplayWanderState();
        }

        void TurnRight()
        {
            _state.WanderMode = WanderModes.TurnRight;
            _state.WanderCounter = TURN_COUNT;
            _mainPort.MotionCommand(TURN_POWER, -TURN_POWER);
            DisplayWanderState();
        }

        void VeerLeft()
        {
            _state.WanderMode = WanderModes.VeerLeft;
            _mainPort.MotionCommand(VEER_POWER, DRIVE_POWER);
            DisplayWanderState();
        }

        void VeerRight()
        {
            _state.WanderMode = WanderModes.VeerRight;
            _mainPort.MotionCommand(DRIVE_POWER, VEER_POWER);
            DisplayWanderState();
        }


        // The Wander Behavior
        // This is a State Machine that runs on a timer.
        // It works reasonably well except for the Stinger's fat
        // backside which scrapes on the walls from time to time.
        void Wander(DateTime t)
        {
            // See if we should execute the Wander behavior
            if (_state.WanderEnabled)
            {
                switch (_state.WanderMode)
                {
                    case WanderModes.DriveStraight:
                        // This code does not check for combinations of Left and Right
                        // which should be unnecessary as long as the Front is free
                        if (_state.IRFront < FRONT_THRESHOLD)
                        {
                            // This presents a problem because the front threshold has
                            // been breached, so we need to take evasive action
                            if (_state.IRLeft < LEFT_THRESHOLD &&
                                _state.IRRight < RIGHT_THRESHOLD)
                            {
                                // If both sides are blocked, back up
                                BackUp();
                            }
                            else
                            {
                                // Otherwise, turn towards the clearest direction
                                if (_state.IRLeft < _state.IRRight)
                                    TurnRight();
                                else
                                    TurnLeft();
                            }
                        }
                        else if (_state.IRLeft < LEFT_THRESHOLD)
                        {
                            // Left side obstacle seen
                            VeerRight();
                        }
                        else if (_state.IRRight < RIGHT_THRESHOLD)
                        {
                            // Right side obstacle
                            VeerLeft();
                        }
                        break;

                    case WanderModes.None:
                        // No mode selected, so start driving
                        DriveStraight();
                        break;

                    case WanderModes.BackUp:
                        // Decrement the counter
                        _state.WanderCounter--;
                        // If finished, try something else
                        // Otherwise just fall through and keep backing up
                        if (_state.WanderCounter <= 0)
                        {
                            // Turn in the clearest direction.
                            // DON'T start driving ahead because that is
                            // where we just came from!
                            if (_state.IRLeft < _state.IRRight)
                                TurnRight();
                            else
                                TurnLeft();
                        }
                        break;

                    case WanderModes.VeerLeft:
                        // If veering has cleared the corresponding side,
                        // then drive straight again.
                        // It makes sense to check here if the way
                        // is blocked first, and if so then continue
                        // turning but this time harder.
                        if (_state.IRFront < FRONT_THRESHOLD)
                            TurnLeft();
                        else
                        {
                            if (_state.IRRight > RIGHT_THRESHOLD)
                                DriveStraight();
                        }
                        break;

                    case WanderModes.VeerRight:
                        if (_state.IRFront < FRONT_THRESHOLD)
                            TurnRight();
                        else
                        {
                            if (_state.IRLeft > LEFT_THRESHOLD)
                                DriveStraight();
                        }
                        break;

                    case WanderModes.TurnLeft:
                        _state.WanderCounter--;
                        if (_state.WanderCounter <= 0)
                        {
                            // If front is clear now, then drive ahead
                            // Otherwise keep turning left
                            if (_state.IRFront > FRONT_THRESHOLD)
                                DriveStraight();
                            else
                                TurnLeft();
                        }
                        break;

                    case WanderModes.TurnRight:
                        _state.WanderCounter--;
                        if (_state.WanderCounter <= 0)
                        {
                            // If front is clear now, then drive ahead
                            // Otherwise keep turning right
                            if (_state.IRFront > FRONT_THRESHOLD)
                                DriveStraight();
                            else
                                TurnRight();
                        }
                        break;

                    default:
                        Console.WriteLine("Invalid Wander Mode!");
                        break;

                }
            }

            // If the timer is still running, then kick it off again
            if (_timerRunning)
            {
                // Restart the timer
                Activate(Arbiter.Receive(
                    false,
                    TimeoutPort(WANDER_INTERVAL),
                    Wander)
                );
            }

        }

        #endregion

    }
}
